/**
 * @file power_ctrl.c
 * @brief 底盘功率控制模块，包含超电管理、功率获取及功率限制算法实现
 * @author WeiFan Qiu (qiuweifan0523@faxmail.com)
 * @version 1.0
 * @date 2025-08-08
 * 
 * @copyright Copyright (c) 2025  邱伟凡
 * 
 * @par 修改日志:
 *   - 2025-08-08: 初始版本创建
 * 
 * @bug 待解决问题:
 *   - 超电状态检测逻辑完善
 *   - 功率分配算法优化
 *   - 直接访问超电的结构体内部数据会错位，之前有过，好像是对齐的原因，改了后忘记测了
 *   - 未与QWF_Cmd.h配合使用
 * 	 - 港科的部分有bug，可能是rls的原因，也可能是计算的原因，之前利物浦的出现过类似情况，改了后就好了，但是突然忘了怎么改的了（好像是二次函数还是c语言符号优先级的问题）
 */
#include "power_ctrl.h"
#include "QWF_Math.h"
#include "QWF_Can.h"
#include "chassis.h"
#include "judge.h"
#include "pid.h"
#include "ctrl.h"
#include "state.h"
/**
 * @brief 电机转矩电流转换为实际电流的系数
 */
#define it_to_ireal_M3508	20.0f/16384
#define i_to_t_M3508		0.3f/(3591.0f/187.0f)
#define it_to_t_M3508		it_to_ireal_M3508*i_to_t_M3508
#define it_to_ireal_gm6020	3.0f/16384
#define i_to_t_gm6020		0.741f
#define it_to_t_gm6020		it_to_ireal_gm6020*i_to_t_gm6020
/*超电*/
/**
 * @brief 超电管理静态实例，存储超电发送/接收数据及状态
 */
static super_elec_t Super_Elec;
super_elec_t *super_elec=&Super_Elec;
extern int32_t persent;
/**
 * @brief 超电数据发送函数
 * @details 向超电模块发送功率限制值（通过CAN2发送到0x210ID）
 * @note 功率限制值乘以100转换为uint16_t格式发送给超电
 */
void super_elec_tx_func(void) // 超电容数据发送
{
	super_elec->tx.PowerLimint=(uint16_t)(chassis_power_ctrl->p_max*100);//REF_Report_CHAS_MaxPower()*100;
	super_elec->tx.data[0] = super_elec->tx.PowerLimint;
	super_elec->tx.data[1] = super_elec->tx.PowerLimint>>8;
	Can_Send_Data(0x210,1,super_elec->tx.data,&get_can_type(h,can2));
}
/**
 * @brief 超电数据接收回调函数
 * @param[in] data CAN接收的数据缓冲区（8字节）
 * @details 解析超电电压、电流数据，计算超电功率、电池功率及总功率
 * @note 超电容量百分比通过超电电压/23估算（23V为满电电压）
 */
void super_elec_rx_func(uint8_t* data)
{		
	super_elec->rx=*(cap_rx_t*)(data);
	super_elec->rx.cap_p=super_elec->rx.cap_i*super_elec->rx.cap_v/10000.f;
	super_elec->rx.bat_p=super_elec->rx.bat_i*super_elec->rx.bat_v/10000.f;
	super_elec->rx.all_p=super_elec->rx.bat_p+super_elec->rx.cap_p;
	super_elec->rx.persent=super_elec->rx.cap_v/23;
	persent=super_elec->rx.cap_v/23;
//			memcpy(&super_elec->rx.SuperCapEnergy,data+2,sizeof(uint16_t));
	super_elec->lose_time=0;
}
/*功率计*/
/**
 * @var Power_Get
 * @brief 功率计数据静态实例，存储实时电压、电流、功率值
 * @note 通过power_get指针提供外部访问接口
 */
static power_get_t Power_Get;
power_get_t* power_get=&Power_Get;
/**
 * @brief 功率计数据解析函数（CAN ID 0x213）
 * @param[in] data CAN接收的数据缓冲区（4字节）
 * @details 解析电压（data[0:1]）和电流（data[2:3]），计算实时功率
 * @note 电压/电流值除以100转换为实际值（单位：V/A）
 */
void power_get_func(uint8_t* data){// 功率计数据解析（ID 0x213）
	power_get->v=(float)((int32_t)(data[1]<<8)|(int32_t)(data[0]))/100.0f;
	power_get->i=(float)((int32_t)(data[3]<<8)|(int32_t)(data[2]))/100.0f;
	power_get->p=power_get->i*power_get->v;
}
/*功率控制*/
#if use_power_model==use_power_model_gangke  // 港科功率模型配置
/**
 * @var Chasis_Power_Ctrl_Struct
 * @brief 港科配置实例，包含各电机功率计算参数及分配系数
 * @details 舵机（GM6020）与轮机（M3508）使用不同的功率计算系数
 */
chassis_power_ctrl_t Chasis_Power_Ctrl_Struct={
.motor_power_ctrl[f_wheel]	={.k_w=0.2103,.k_t2=5.732,.k_0=4.35},
.motor_power_ctrl[f_helm]	={.k_w=0.2103,.k_t2=5.732,.k_0=4.35},
.motor_power_ctrl[b_wheel]	={.k_w=0.2103,.k_t2=5.732,.k_0=4.35},
.motor_power_ctrl[b_helm]	={.k_w=0.2103,.k_t2=5.732,.k_0=4.35},
.motor_power_ctrl[l_wheel]	={.k_w=0.2103,.k_t2=5.732,.k_0=4.35},
.motor_power_ctrl[r_wheel]	={.k_w=0.2103,.k_t2=5.732,.k_0=4.35},
.E_upper=500,
.E_lower=100
};
chassis_power_ctrl_t *chassis_power_ctrl=&Chasis_Power_Ctrl_Struct;
/**
 * @brief 电机功率更新宏
 * @param PID_Motor 电机PID指针
 * @param motor_where 电机位置（枚举值）
 * @param it_to_t 时间转换系数（用于将PID输出转换为电机速度）
 * @details 计算电机模型功率（包括速度、转矩、电压），并更新功率控制参数
 */
#define motor_power_update(PID_Motor,motor_where,it_to_t)	\
	chassis_power_ctrl->motor_power_ctrl[motor_where].pid=PID_Motor;\
	chassis_power_ctrl->motor_power_ctrl[motor_where].t=chassis_power_ctrl->motor_power_ctrl[motor_where].pid->output*it_to_t;\
	chassis_power_ctrl->motor_power_ctrl[motor_where].w=chassis_power_ctrl->motor_power_ctrl[motor_where].pid->measure[now];\
	chassis_power_ctrl->motor_power_ctrl[motor_where].p=chassis_power_ctrl->motor_power_ctrl[motor_where].t*chassis_power_ctrl->motor_power_ctrl[motor_where].w\
													+chassis_power_ctrl->motor_power_ctrl[motor_where].k_w*QWF_Esp(QWF_rpm_to_rad(chassis_power_ctrl->motor_power_ctrl[motor_where].w),0)\
													+chassis_power_ctrl->motor_power_ctrl[motor_where].k_t2*chassis_power_ctrl->motor_power_ctrl[motor_where].t*chassis_power_ctrl->motor_power_ctrl[motor_where].t+chassis_power_ctrl->motor_power_ctrl[motor_where].k_0;

/**
 * @brief 底盘功率控制主函数（港科模型）
 * @details 1. 计算各电机实时功率
 *          2. 总功率超过限制时，通过二次方程求解电机输出限制值
 *          3. 根据误差系数调整功率分配比例
 */
void Chassis_Power_Control(void){
	chassis_power_ctrl->p_max=40;
	motor_power_update(chassis_pid->f.v,f_wheel,it_to_ireal_M3508*i_to_t_M3508)
	motor_power_update(chassis_pid->f.w,f_helm,it_to_ireal_gm6020*i_to_t_gm6020)
	motor_power_update(chassis_pid->b.v,b_wheel,it_to_ireal_M3508*i_to_t_M3508)
	motor_power_update(chassis_pid->b.w,b_helm,it_to_ireal_gm6020*i_to_t_gm6020)
	motor_power_update(chassis_pid->l.v,l_wheel,it_to_ireal_M3508*i_to_t_M3508)
	motor_power_update(chassis_pid->r.v,r_wheel,it_to_ireal_M3508*i_to_t_M3508)
	chassis_power_ctrl->p_all=0;
	chassis_power_ctrl->p_all_with_sign=0;//带符号p，用于rls计算
	for(uint8_t i=0;i<chassis_wheel_num;i++){
		if(chassis_power_ctrl->motor_power_ctrl[i].p>0)
			chassis_power_ctrl->p_all+=chassis_power_ctrl->motor_power_ctrl[i].p;
		chassis_power_ctrl->p_all_with_sign+=chassis_power_ctrl->motor_power_ctrl[i].p;
	}
	 chassis_power_ctrl->err_all=chassis_power_ctrl->motor_power_ctrl[f_wheel].pid->err[now];
	chassis_power_ctrl->err_all+=chassis_power_ctrl->motor_power_ctrl[f_helm].pid->err[now];
	chassis_power_ctrl->err_all+=chassis_power_ctrl->motor_power_ctrl[b_wheel].pid->err[now];
	chassis_power_ctrl->err_all+=chassis_power_ctrl->motor_power_ctrl[b_helm].pid->err[now];
	chassis_power_ctrl->err_all+=chassis_power_ctrl->motor_power_ctrl[l_wheel].pid->err[now];
	chassis_power_ctrl->err_all+=chassis_power_ctrl->motor_power_ctrl[r_wheel].pid->err[now];
		chassis_power_ctrl->k_coe=(chassis_power_ctrl->err_all-chassis_power_ctrl->E_lower)/(chassis_power_ctrl->E_upper-chassis_power_ctrl->E_lower);	
		if(chassis_power_ctrl->p_all>chassis_power_ctrl->p_max)//超功率，进行约束
		{
			for(uint8_t i=0;i<chassis_wheel_num;i++){
				chassis_power_ctrl->motor_power_ctrl[i].p_max=chassis_power_ctrl->k_coe*chassis_power_ctrl->motor_power_ctrl[i].pid->err[now]/chassis_power_ctrl->err_all+(1-chassis_power_ctrl->k_coe)*chassis_power_ctrl->motor_power_ctrl[i].p/chassis_power_ctrl->p_all_with_sign;
				chassis_power_ctrl->motor_power_ctrl[i].a=chassis_power_ctrl->motor_power_ctrl[i].k_t2;
				chassis_power_ctrl->motor_power_ctrl[i].b=chassis_power_ctrl->motor_power_ctrl[i].w;
				chassis_power_ctrl->motor_power_ctrl[i].c=chassis_power_ctrl->motor_power_ctrl[i].k_t2*QWF_Esp(chassis_power_ctrl->motor_power_ctrl[i].w,0)+chassis_power_ctrl->motor_power_ctrl[i].k_0-chassis_power_ctrl->motor_power_ctrl[i].p;
				chassis_power_ctrl->motor_power_ctrl[i].derta=chassis_power_ctrl->motor_power_ctrl[i].b*chassis_power_ctrl->motor_power_ctrl[i].b-4*chassis_power_ctrl->motor_power_ctrl[i].a*chassis_power_ctrl->motor_power_ctrl[i].c;
				if(chassis_power_ctrl->motor_power_ctrl[i].derta<0)
					chassis_power_ctrl->motor_power_ctrl[i].pid->output=-1*chassis_power_ctrl->motor_power_ctrl[i].b/(2*chassis_power_ctrl->motor_power_ctrl[i].k_t2);
				else if(chassis_power_ctrl->motor_power_ctrl[i].derta==0)
					chassis_power_ctrl->motor_power_ctrl[i].pid->output=-1*chassis_power_ctrl->motor_power_ctrl[i].b/(2*chassis_power_ctrl->motor_power_ctrl[i].a);
				else if(chassis_power_ctrl->motor_power_ctrl[i].derta>0)
				{
					if(chassis_power_ctrl->motor_power_ctrl[i].pid->output>0)
						chassis_power_ctrl->motor_power_ctrl[i].pid->output=(-1*chassis_power_ctrl->motor_power_ctrl[i].b+sqrt(chassis_power_ctrl->motor_power_ctrl[i].derta))/(2*chassis_power_ctrl->motor_power_ctrl[i].a);
					else if(chassis_power_ctrl->motor_power_ctrl[i].pid->output<0)
						chassis_power_ctrl->motor_power_ctrl[i].pid->output=(-1*chassis_power_ctrl->motor_power_ctrl[i].b-sqrt(chassis_power_ctrl->motor_power_ctrl[i].derta))/(2*chassis_power_ctrl->motor_power_ctrl[i].a);
					else 
						chassis_power_ctrl->motor_power_ctrl[i].pid->output=0;
				}
			}
		}
}
#elif 	use_power_model==use_power_model_liwupu  // 利物浦功率模型配置
/**
 * @brief 利物浦功率模型配置
 */
chassis_power_ctrl_t Chasis_Power_Ctrl_Struct={
.my_k_p_max=0.67,
.motor_power_ctrl[f_wheel]=	{.k_it_w=it_to_t_M3508 /9.55f,.k_w2=1.453e-07,.k_it2=2.53e-07,.k_0= 4.081f/4.0f},
.motor_power_ctrl[f_helm ]= {.k_it_w=it_to_t_gm6020/9.55f,.k_w2=1.453e-07,.k_it2=2.53e-07,.k_0= 12.081f/2.0f},
.motor_power_ctrl[b_wheel]=	{.k_it_w=it_to_t_M3508 /9.55f,.k_w2=1.453e-07,.k_it2=2.53e-07,.k_0= 4.081f/4.0f},
.motor_power_ctrl[b_helm ]=	{.k_it_w=it_to_t_gm6020/9.55f,.k_w2=1.453e-07,.k_it2=2.53e-07,.k_0= 12.081f/2.0f},
.motor_power_ctrl[l_wheel]=	{.k_it_w=it_to_t_M3508 /9.55f,.k_w2=1.453e-07,.k_it2=2.53e-07,.k_0= 4.081f/4.0f},
.motor_power_ctrl[r_wheel]=	{.k_it_w=it_to_t_M3508 /9.55f,.k_w2=1.453e-07,.k_it2=2.53e-07,.k_0= 4.081f/4.0f}

};
chassis_power_ctrl_t *chassis_power_ctrl=&Chasis_Power_Ctrl_Struct;

/**
 * @brief 电机功率更新宏（利物浦模型）
 * @param PID_Motor 电机PID指针
 * @param motor_where 电机位置（枚举值）
 * @details 计算电机模型功率（包括速度、转矩、电压），并更新功率控制参数
 */
#define motor_power_update(PID_Motor,motor_where)	\
	chassis_power_ctrl->motor_power_ctrl[motor_where].pid=&PID_Motor;\
	chassis_power_ctrl->motor_power_ctrl[motor_where].it=chassis_power_ctrl->motor_power_ctrl[motor_where].pid->output;\
	chassis_power_ctrl->motor_power_ctrl[motor_where].w=chassis_power_ctrl->motor_power_ctrl[motor_where].pid->measure[now];\
	chassis_power_ctrl->motor_power_ctrl[motor_where].p=chassis_power_ctrl->motor_power_ctrl[motor_where].k_it_w*chassis_power_ctrl->motor_power_ctrl[motor_where].w*chassis_power_ctrl->motor_power_ctrl[motor_where].it\
													+chassis_power_ctrl->motor_power_ctrl[motor_where].k_w2*chassis_power_ctrl->motor_power_ctrl[motor_where].w*chassis_power_ctrl->motor_power_ctrl[motor_where].w\
													+chassis_power_ctrl->motor_power_ctrl[motor_where].k_it2*chassis_power_ctrl->motor_power_ctrl[motor_where].it*chassis_power_ctrl->motor_power_ctrl[motor_where].it+chassis_power_ctrl->motor_power_ctrl[motor_where].k_0;
/**
 * @brief 功率分配宏（利物浦模型）
 * @param motor_p_max 电机最大功率
 * @param motor_where 电机位置（枚举值）
 * @details 根据电机模型计算功率，根据功率分配比例调整输出
 */
#define power_allot(motor_p_max,motor_where)\
			if(chassis_power_ctrl->motor_power_ctrl[motor_where].p>0){\
				chassis_power_ctrl->motor_power_ctrl[motor_where].p_max=motor_p_max;\
				chassis_power_ctrl->motor_power_ctrl[motor_where].a=chassis_power_ctrl->motor_power_ctrl[motor_where].k_it2;\
				chassis_power_ctrl->motor_power_ctrl[motor_where].b=chassis_power_ctrl->motor_power_ctrl[motor_where].k_it_w*chassis_power_ctrl->motor_power_ctrl[motor_where].w;\
				chassis_power_ctrl->motor_power_ctrl[motor_where].c=chassis_power_ctrl->motor_power_ctrl[motor_where].k_w2*chassis_power_ctrl->motor_power_ctrl[motor_where].w*chassis_power_ctrl->motor_power_ctrl[motor_where].w-chassis_power_ctrl->motor_power_ctrl[motor_where].p_max+chassis_power_ctrl->motor_power_ctrl[motor_where].k_0;\
				chassis_power_ctrl->motor_power_ctrl[motor_where].derta=derta(chassis_power_ctrl->motor_power_ctrl[motor_where].a,chassis_power_ctrl->motor_power_ctrl[motor_where].b,chassis_power_ctrl->motor_power_ctrl[motor_where].c);\
				if(chassis_power_ctrl->motor_power_ctrl[motor_where].derta<0)\
					chassis_power_ctrl->motor_power_ctrl[motor_where].pid->output=0;\
				/*else if(chassis_power_ctrl->motor_power_ctrl[motor_where].derta==0)\
					chassis_power_ctrl->motor_power_ctrl[motor_where].pid->output=x2_func_x_plus(chassis_power_ctrl->motor_power_ctrl[motor_where].a,chassis_power_ctrl->motor_power_ctrl[motor_where].b,chassis_power_ctrl->motor_power_ctrl[motor_where].c);*/\
				else if(chassis_power_ctrl->motor_power_ctrl[motor_where].derta>=0)\
				{\
					if(chassis_power_ctrl->motor_power_ctrl[motor_where].pid->output>0)\
						chassis_power_ctrl->motor_power_ctrl[motor_where].pid->output=x2_func_x_plus(chassis_power_ctrl->motor_power_ctrl[motor_where].a,chassis_power_ctrl->motor_power_ctrl[motor_where].b,chassis_power_ctrl->motor_power_ctrl[motor_where].c);\
					else if(chassis_power_ctrl->motor_power_ctrl[motor_where].pid->output<0)\
						chassis_power_ctrl->motor_power_ctrl[motor_where].pid->output=x2_func_x_sub(chassis_power_ctrl->motor_power_ctrl[motor_where].a,chassis_power_ctrl->motor_power_ctrl[motor_where].b,chassis_power_ctrl->motor_power_ctrl[motor_where].c);\
					else \
						chassis_power_ctrl->motor_power_ctrl[motor_where].pid->output=0;\
				}\
			}

extern int32_t persent;

/**
 * @brief 底盘功率控制主函数（利物浦模型）
 * @details 1. 根据裁判系统状态设置最大允许功率
 *          2. 超电可用时提升功率上限
 *          3. 计算各电机实时功率并求和
 *          4. 超功率时按比例分配功率限制（舵机优先）
 */
void Chassis_Power_Control(void)
{
	if((judge->robot_status.chassis_power_limit)&&(judge->game_status.game_progress==4))//在比赛并且收到裁判系统的功率上限值了
		chassis_power_ctrl->p_max=judge->robot_status.chassis_power_limit+PID_Calculate(&chassis_pid->special.power,judge->power_heat_data.buffer_energy,30);
	else if(judge->robot_status.chassis_power_limit)//不在比赛，但是收到裁判系统的功率上限值
		chassis_power_ctrl->p_max=judge->robot_status.chassis_power_limit+PID_Calculate(&chassis_pid->special.power,judge->power_heat_data.buffer_energy,60);
	else//根本没接裁判系统
		chassis_power_ctrl->p_max=60;
	if(!qwf_state->chassis.super_elec.state){//看一下超点在不在线
		if(		super_elec->use&&persent>=60){//看一下超点容量够再用
			chassis_power_ctrl->p_max=300;
		}
		super_elec_tx_func();//在线就给他发东西		
	}
	/*先算每一个电机的功率值*/
	#if Chassis_Wheel_Type==Helf_Helm_Wheel
	motor_power_update(chassis_pid->f.wheel.v,f_wheel)
	motor_power_update(chassis_pid->f.helm .w,f_helm)
	motor_power_update(chassis_pid->b.wheel.v,b_wheel)
	motor_power_update(chassis_pid->b.helm .w,b_helm)
	motor_power_update(chassis_pid->l.wheel.v,l_wheel)
	motor_power_update(chassis_pid->r.wheel.v,r_wheel)
	chassis_power_ctrl->p_all=0;//先清零，防止奇奇怪怪的错误疯车	
	for(uint8_t i=0;i<chassis_wheel_num;i++){//求和
		if(chassis_power_ctrl->motor_power_ctrl[i].p>0)//负功率不算{计为0[其实是算成给模型精确度的余量（因为模型不可能100%精确)]}
			chassis_power_ctrl->p_all+=chassis_power_ctrl->motor_power_ctrl[i].p;
	}
	if(chassis_power_ctrl->p_all>chassis_power_ctrl->p_max){//超出最大功率限制值，进行约束	
		/*先分别算出舵向和轮向所需功率（负功率的算成0）*/
		chassis_power_ctrl->p_6020= (chassis_power_ctrl->motor_power_ctrl[f_helm ].p>0?chassis_power_ctrl->motor_power_ctrl[f_helm ].p:0)+\
									(chassis_power_ctrl->motor_power_ctrl[b_helm ].p>0?chassis_power_ctrl->motor_power_ctrl[b_helm ].p:0);
		chassis_power_ctrl->p_c620= (chassis_power_ctrl->motor_power_ctrl[f_wheel].p>0?chassis_power_ctrl->motor_power_ctrl[f_wheel].p:0)+\
									(chassis_power_ctrl->motor_power_ctrl[b_wheel].p>0?chassis_power_ctrl->motor_power_ctrl[b_wheel].p:0)+\
									(chassis_power_ctrl->motor_power_ctrl[l_wheel].p>0?chassis_power_ctrl->motor_power_ctrl[l_wheel].p:0)+\
									(chassis_power_ctrl->motor_power_ctrl[r_wheel].p>0?chassis_power_ctrl->motor_power_ctrl[r_wheel].p:0);	
		if(chassis_power_ctrl->p_6020>=chassis_power_ctrl->p_max*chassis_power_ctrl->my_k_p_max){//如果6020超出总功率中预设置分给6020的量
			/*舵限制*/
			power_allot(chassis_power_ctrl->p_max*chassis_power_ctrl->my_k_p_max/*舵可用功率*/*(chassis_power_ctrl->motor_power_ctrl[f_helm].p/chassis_power_ctrl->p_6020),f_helm)
			power_allot(chassis_power_ctrl->p_max*chassis_power_ctrl->my_k_p_max/*舵可用功率*/*(chassis_power_ctrl->motor_power_ctrl[b_helm ].p/chassis_power_ctrl->p_6020) ,b_helm)
			/*剩下给轮*/
			power_allot(chassis_power_ctrl->p_max*(1-chassis_power_ctrl->my_k_p_max)/*轮子可用功率*/*chassis_power_ctrl->motor_power_ctrl[f_wheel].p/chassis_power_ctrl->p_c620,f_wheel  )
			power_allot(chassis_power_ctrl->p_max*(1-chassis_power_ctrl->my_k_p_max)/*轮子可用功率*/*chassis_power_ctrl->motor_power_ctrl[b_wheel ].p/chassis_power_ctrl->p_c620,b_wheel )
			power_allot(chassis_power_ctrl->p_max*(1-chassis_power_ctrl->my_k_p_max)/*轮子可用功率*/*chassis_power_ctrl->motor_power_ctrl[l_wheel ].p/chassis_power_ctrl->p_c620,l_wheel   )
			power_allot(chassis_power_ctrl->p_max*(1-chassis_power_ctrl->my_k_p_max)/*轮子可用功率*/*chassis_power_ctrl->motor_power_ctrl[r_wheel].p/chassis_power_ctrl->p_c620,r_wheel  )
//			Q.p_out1=chassis_power_ctrl->p_max*chassis_power_ctrl->my_k_p_max/*舵可用功率*/	  *(chassis_power_ctrl->motor_power_ctrl[f_helm].p/chassis_power_ctrl->p_6020);
//			Q.p_out2=chassis_power_ctrl->p_max*chassis_power_ctrl->my_k_p_max/*舵可用功率*/	  *(chassis_power_ctrl->motor_power_ctrl[b_helm ].p/chassis_power_ctrl->p_6020);
//			Q.p_out3=chassis_power_ctrl->p_max*(1-chassis_power_ctrl->my_k_p_max)/*轮子可用功率*/*chassis_power_ctrl->motor_power_ctrl[f_wheel   ].p/chassis_power_ctrl->p_c620;
//			Q.p_out4=chassis_power_ctrl->p_max*(1-chassis_power_ctrl->my_k_p_max)/*轮子可用功率*/*chassis_power_ctrl->motor_power_ctrl[b_wheel    ].p/chassis_power_ctrl->p_c620;
//			Q.p_out5=chassis_power_ctrl->p_max*(1-chassis_power_ctrl->my_k_p_max)/*轮子可用功率*/*chassis_power_ctrl->motor_power_ctrl[l_wheel    ].p/chassis_power_ctrl->p_c620;
//			Q.p_out6=chassis_power_ctrl->p_max*(1-chassis_power_ctrl->my_k_p_max)/*轮子可用功率*/*chassis_power_ctrl->motor_power_ctrl[r_wheel   ].p/chassis_power_ctrl->p_c620;
		}
		else{//6020用不完预先分给他的，就不限6020，只限制其他的（其他的要注意必须减去6020的）
		/*舵不限*/
		/*轮分剩下的*/
			power_allot((chassis_power_ctrl->p_max-chassis_power_ctrl->p_6020)*chassis_power_ctrl->motor_power_ctrl[f_wheel].p/chassis_power_ctrl->p_c620,f_wheel  )
			power_allot((chassis_power_ctrl->p_max-chassis_power_ctrl->p_6020)*chassis_power_ctrl->motor_power_ctrl[b_wheel ].p/chassis_power_ctrl->p_c620,b_wheel )
			power_allot((chassis_power_ctrl->p_max-chassis_power_ctrl->p_6020)*chassis_power_ctrl->motor_power_ctrl[l_wheel ].p/chassis_power_ctrl->p_c620,l_wheel   )
			power_allot((chassis_power_ctrl->p_max-chassis_power_ctrl->p_6020)*chassis_power_ctrl->motor_power_ctrl[r_wheel].p/chassis_power_ctrl->p_c620,r_wheel  )
		}
	}
	#elif Chassis_Wheel_Type==Omni_Wheel||Chassis_Wheel_Type==Mecanum_Wheel
	#elif Chassis_Wheel_Type==Leg_Wheel
	#endif
}
#endif
